import numpy as np
import control

class System:
    def __init__(self, mass, Ixx, Iyy, Izz, g):
        self.mass = mass
        self.Ixx = Ixx
        self.Iyy = Iyy
        self.Izz = Izz
        self.g = g

    def get_dLTI(self, dt):
        num_x = 12
        Ac = np.array([[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
                       [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
                       [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
                       [0, 0, 0, 0, 0, 0, 0, self.g, 0, 0, 0, 0],
                       [0, 0, 0, 0, 0, 0, (-self.g), 0, 0, 0, 0, 0],
                       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
                       [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
                       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
                       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
                       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
                       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
                       [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], ])
        Bc = np.array([[0, 0, 0, 0],
                       [0, 0, 0, 0],
                       [0, 0, 0, 0],
                       [0, 0, 0, 0],
                       [0, 0, 0, 0],
                       [-4.17945 / self.mass, -4.17945 / self.mass, -4.17945 / self.mass, -4.17945 / self.mass],
                       [0, 0, 0, 0],
                       [0, 0, 0, 0],
                       [0, 0, 0, 0],
                       [-0.67233/self.Ixx, 0.67233/self.Ixx, 0.67233/self.Ixx, -0.67233/self.Ixx],
                       [0.67233/self.Iyy, -0.67233/self.Iyy, 0.67233/self.Iyy, -0.67233/self.Iyy],
                       [0.05562/self.Izz, 0.05562/self.Izz, -0.05562/self.Izz, -0.05562/self.Izz]])
        Cc = np.array([[1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
                       [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
                       [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
                       [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
                       [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
                       [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0]])
        Dc = np.zeros((6, 4))
        sysc = control.ss(Ac, Bc, Cc, Dc)
        # Discretization 双线性变换离散化
        sysd = control.sample_system(sysc, dt, method='bilinear')
        print(Bc)
        return sysd.A, sysd.B

# 创建系统实例
system = System(mass=1.0, Ixx=0.00089, Iyy=0.00221, Izz=0.00289, g=9.8)
# 离散化时间间隔
dt = 0.1
# 获取离散时间系统的状态矩阵和输入矩阵
A, B = system.get_dLTI(dt)
print("离散时间系统的状态矩阵 A:")
print(A)
print("离散时间系统的输入矩阵 B:")
print(B)